#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"
#include "RASLib/servo.h"
#include "RobzDemo.h"
 
void initMotors(void);
void initLineSensor(void);
void initUART(void);
void initIRSensor(void);
void initEncoders(void);
void Wait(long ms);
void initServo(void);
void SetDischargeTime(int ms);
long getADCValue(void);
unsigned char ReadLineSensor(void);
void UARTprintf(const char *pcString, ...);
long PIDquicktime(long position);

long distance;
long Kp=15,Ki=0,Kd=27;	   //15 0 27
long currentError,lastError;
long integral=0;

void quicktime(void){
	//Encoders
	signed long encoder0, encoder1;
	signed long encoderDuration=800;

	signed char speed=127;
	//long motorOffset=16;

	//Random variables 	  
	long MV;
	long MVabs=0;
	signed char velocity;
	unsigned char servoPosition=255;
	//quicktime values
	signed long lookForLine=0;
	signed long goingBack=0;
	char lineSensorByte;

	LockoutProtection();
	InitializeMCU();
	initIRSensor();
	initEncoders();
	initMotors();
	initLineSensor();
	initUART();
	initServo();
	SetDischargeTime(220);
	PresetEncoderCounts(0,0);
						  
	SetServoPosition(SERVO_0, servoPosition);
	Wait(2000);			   
	SetMotorPowers(speed,speed-motorOffset);
	distance=getADCValue();
	Wait(10);
	distance/=20;

	while(1){
		GetEncoderCounts(&encoder0, &encoder1);
		if((encoder0 > encoderDuration) && (encoder1 > encoderDuration)&&!goingBack) {
			lookForLine=1;
		}
		if(lookForLine) {
			lineSensorByte = ReadLineSensor();
		}
		if(lookForLine&&lineSensorByte!=255) {	
			Wait(900);
			SetMotorPowers(0,0);
			Wait(200);
			goingBack=1;
			lookForLine=0;
		}
		MV = PIDquicktime(getADCValue()); 
		Wait(10);
		if(MV>255)MV=255;
		if(MV*-1>255)MV=-255;
		//If MV is positive, the robot needs to go left. If MV is negative, the robot needs to go right
		//But... we first need to come up with a linear relationship relating MV to the speed of the
		//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
		MVabs=MV;
		if(MV<0)MVabs*=-1;
		velocity=(signed char)(speed-MVabs);
		if(velocity<-128)velocity=-128;	
		UARTprintf("%d   %d   %d   %d\r",velocity,encoder0,encoder1,integral);
		if(MV>0){
			if(goingBack)SetMotorPowers(-speed,-velocity);
			else SetMotorPowers(speed,velocity);
		}
		else if(MV<0){
			if(goingBack)SetMotorPowers(-velocity,-speed+motorOffset);
			else SetMotorPowers(velocity,speed-motorOffset);
		}
		else{//MV equals 0; never going to happen, but might?
			if(goingBack)SetMotorPowers(-speed,-speed+motorOffset);
			else SetMotorPowers(speed,speed-motorOffset);
		}	  
	}
	//SetMotorPowers(0,0);
}

long PIDquicktime(long position){
	//PID variables 	 
	signed long derivative=0;
    signed long Pout;
	signed long Iout;
	signed long Dout;
	signed long MV=0;
	
	lastError=currentError;
	//Now it's time to compute the error...
	currentError = (distance - (position / 20));
	//Now that we have the error, we can update the derivative and integral
	derivative=currentError-lastError;
	integral+=currentError/8;
	//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
	//It should be noted that a positive derivative means the robot needs to turn left, and a
	//negative derivative means the robot needs to turn right. A positive integral means the
	//robot needs to skew left, and a negative integral value means the robot needs to skew right
	Pout=Kp*currentError;								   
	Dout=Kd*derivative;
	Iout=Ki*integral;
	MV=Pout+Iout+Dout;
	
	return MV;	 
}
